#include<iostream>
#include<opencv2/imgcodecs.hpp>
#include<opencv2/highgui.hpp>
#include<opencv2/imgproc.hpp>
#include<opencv2/objdetect.hpp>
#include<opencv2/opencv.hpp>
#include<opencv2/ximgproc.hpp>
#include<string>
#include<vector>
#include<fstream>
#include <fcntl.h>
#include <termios.h>
#include"func.h"

using namespace std;
using namespace cv;



void _Gimbal__init()//云台启动函数
{
    Mat img;//原始图像
    Mat target_bin;//目标颜色二值化图像
    Mat toward_bin;//枪口指向二值化图像
    VideoCapture cap(2);//获取云台相机图像
    CascadeClassifier faceCasade;//人脸识别
    vector<Rect> faces;//存储识别结果容器
    int loc[4];//取出result中坐标的数组

    _usart_init__();//初始化串口
    facerecognize_init(faceCasade);//初始化人脸识别

    
    

    while(1)
    {
        cap.read(img);//读取云台相机图像
        
        //对获取的二值化图像进行识别，以vec形式返回目标和激光点坐标
        Result result;
        // result=recognize(target_bin,toward_bin,img);
        facerecognize(img,faces,faceCasade,result);
        Point center(img.cols/2, img.rows/2);
        circle(img, center, 50, cv::Scalar(0, 0, 255), 2);
        circle(img, center, 5, cv::Scalar(0, 0, 255), -1);


        //取出vec中坐标，放入loc数组中
        loc[0]=result.targetX;
        loc[1]=result.targetY;
        loc[2]=img.cols/2;//瞄准.x
        loc[3]=img.rows/2;//瞄准.y
        cout << "Laser point: (" << img.cols/2 << ", " << img.rows/2 << ")" << endl;

        //通过串口向单片机发送坐标
        _usart_send__(loc);

        imshow("img",img);

        waitKey(1);

    }
}
